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 __init__(self): ik_serv_name = '/compute_ik' fk_serv_name = '/compute_fk' self.frame_id = "torso_lift_link" self.r_link_name = 'r_wrist_roll_link' self.l_link_name = 'l_wrist_roll_link' self.r_group_name = 'right_arm' self.l_group_name = 'left_arm' #Set up right/left FK service connections print 'Waiting for forward kinematics services...' rospy.wait_for_service(fk_serv_name) self.getPosFK = rospy.ServiceProxy(fk_serv_name, GetPositionFK) self.getPosFKPersistent = rospy.ServiceProxy(fk_serv_name, GetPositionFK, persistent=True) print "OK" #Set up right/left FK service requests self.FKreq = [GetPositionFKRequest(), GetPositionFKRequest()] self.FKreq[0].header.frame_id = self.frame_id self.FKreq[0].fk_link_names = [self.r_link_name] self.FKreq[0].robot_state.joint_state.name = self.getJointNames(0) self.FKreq[1].header.frame_id = self.frame_id self.FKreq[1].fk_link_names = [self.l_link_name] self.FKreq[1].robot_state.joint_state.name = self.getJointNames(1) #Set up right/left IK service connections print 'Waiting for inverse kinematics services...' rospy.wait_for_service(ik_serv_name) self.getPosIK = rospy.ServiceProxy(ik_serv_name, GetPositionIK) self.getPosIKPersistent = rospy.ServiceProxy(ik_serv_name, GetPositionIK, persistent=True) print 'OK' #Set up right/left IK service requests self.IKreq = [GetPositionIKRequest(), GetPositionIKRequest()] self.IKreq[0].ik_request.robot_state.joint_state.position = [0] * 7 self.IKreq[ 0].ik_request.robot_state.joint_state.name = self.getJointNames(0) self.IKreq[0].ik_request.group_name = self.r_group_name self.IKreq[0].ik_request.ik_link_name = self.r_link_name self.IKreq[0].ik_request.pose_stamped.header.frame_id = self.frame_id self.IKreq[1].ik_request.robot_state.joint_state.position = [0] * 7 self.IKreq[ 1].ik_request.robot_state.joint_state.name = self.getJointNames(1) self.IKreq[1].ik_request.group_name = self.l_group_name self.IKreq[1].ik_request.ik_link_name = self.l_link_name self.IKreq[1].ik_request.pose_stamped.header.frame_id = self.frame_id
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)): """Computes inverse kinematics for the given pose. Note: if you are interested in returning the IK solutions, we have shown how to access them. Args: pose_stamped: geometry_msgs/PoseStamped. timeout: rospy.Duration. How long to wait before giving up on the IK solution. Returns: A list of (name, value) for the arm joints if the IK solution was found, False otherwise. """ request = GetPositionIKRequest() request.ik_request.pose_stamped = pose_stamped request.ik_request.group_name = 'arm' request.ik_request.timeout = timeout response = self._compute_ik(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: return False joint_state = response.solution.joint_state joints = [] for name, position in zip(joint_state.name, joint_state.position): if name in ArmJoints.names(): joints.append((name, position)) return joints
def getIkPose(self, pose, group="right_arm", previous_state=None): """Get IK of the pose specified, for the group specified, optionally using the robot_state of previous_state (if not, current robot state will be requested) """ # point point to test if there is ik # returns the answer of the service rqst = GetPositionIKRequest() rqst.ik_request.avoid_collisions = True rqst.ik_request.group_name = group rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now()) rqst.ik_request.pose_stamped.header.frame_id = 'base_link' # Set point to check IK for rqst.ik_request.pose_stamped.pose.position = pose.position rqst.ik_request.pose_stamped.pose.orientation = pose.orientation if previous_state == None: current_joint_state = rospy.wait_for_message( DEFAULT_JOINT_STATES, JointState) cs = RobotState() cs.joint_state = current_joint_state #cs = self.r_commander.get_current_state() # old code rqst.ik_request.robot_state = cs else: rqst.ik_request.robot_state = previous_state ik_answer = GetPositionIKResponse() if DEBUG_MODE: timeStart = rospy.Time.now() ik_answer = self.ik_serv.call(rqst) if DEBUG_MODE: durationCall = rospy.Time.now() - timeStart rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s") return ik_answer
def getIK(self, group_name, ik_link_name, pose, avoid_collisions=True, attempts=None, robot_state=None, constraints=None): """Get the inverse kinematics for a group with a link a in pose in 3d world. @group_name string group i.e. right_arm that will perform the IK @ik_link_name string link that will be in the pose given to evaluate the IK @pose PoseStamped that represents the pose (with frame_id!) of the link @avoid_collisions Bool if we want solutions with collision avoidance @attempts Int number of attempts to get an Ik as it can fail depending on what IK is being used @robot_state RobotState the robot state where to start searching IK from (optional, current pose will be used if ignored)""" gpikr = GetPositionIKRequest() gpikr.ik_request.group_name = group_name if robot_state != None: # current robot state will be used internally otherwise gpikr.ik_request.robot_state = robot_state gpikr.ik_request.avoid_collisions = avoid_collisions gpikr.ik_request.ik_link_name = ik_link_name if type(pose) == type(PoseStamped()): gpikr.ik_request.pose_stamped = pose else: rospy.logerr("pose is not a PoseStamped, it's: " + str(type(pose)) + ", can't ask for an IK") return if attempts != None: gpikr.ik_request.attempts = attempts else: gpikr.ik_request.attempts = 0 if constraints != None: gpikr.ik_request.constraints = constraints ik_result = self.ik_srv.call(gpikr) rospy.logwarn("Sent: " + str(gpikr)) return ik_result
def computeIKPossible(self, x, y, z, qx=0.707, qy=0.707, qz=0.0, qw=0.0): req = GetPositionIKRequest() req.ik_request.group_name = "manipulator" req.ik_request.robot_state = RobotState() # req.ik_request.robot_state.joint_state = self.joint_state req.ik_request.avoid_collisions = True req.ik_request.pose_stamped = PoseStamped() req.ik_request.pose_stamped.header.frame_id = "base_link" req.ik_request.pose_stamped.header.stamp = rospy.get_rostime() 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 = qx req.ik_request.pose_stamped.pose.orientation.y = qy req.ik_request.pose_stamped.pose.orientation.z = qz req.ik_request.pose_stamped.pose.orientation.w = qw req.ik_request.timeout = rospy.Duration(0.05) try: serviceCallLink = rospy.ServiceProxy('/compute_ik', GetPositionIK) response = serviceCallLink(req) # print response if response.error_code.val is not 1: # if nost success then issue print "No solution at %0.2f, %0.2f, %0.2f" % (x, y, z) else: return True except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
def find_IK_solution(ik, target, seed, group_name): response = ik(GetPositionIKRequest(ik_request = PositionIKRequest( group_name = group_name, pose_stamped = PoseStamped( header = Header(frame_id="base_link"), pose = target), robot_state = RobotState(joint_state=seed)) ) ) return response
def make_single_move(pose): print "ok" s = pose print s #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "ar_marker_3" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = s[0] request.ik_request.pose_stamped.pose.position.y = s[1] request.ik_request.pose_stamped.pose.position.z = s[2] request.ik_request.pose_stamped.pose.orientation.x = s[3] request.ik_request.pose_stamped.pose.orientation.y = s[4] request.ik_request.pose_stamped.pose.orientation.z = s[5] request.ik_request.pose_stamped.pose.orientation.w = s[6] try: print "ok2" response = compute_ik(request) group = MoveGroupCommander("left_arm") print "ok3" group.set_pose_target(request.ik_request.pose_stamped) print "ok4" group.go() print "in" print x #print "out" except rospy.ServiceException, e: print "Service call failed: %s"%e
def get_position_ik(group_name, pose_stamped, ik_link_name=None, robot_state=None): """ """ # Use current robot state if None if robot_state is None: robot_state = get_robot_state() # Prepare request req = GetPositionIKRequest() req.ik_request.group_name = group_name if ik_link_name is not None: req.ik_request.ik_link_name = ik_link_name req.ik_request.pose_stamped = pose_stamped req.ik_request.robot_state = robot_state req.ik_request.avoid_collisions = True # Call IK service ik_srv = rospy.ServiceProxy('compute_ik', GetPositionIK) try: res = ik_srv(req) except rospy.service.ServiceException, e: rospy.logerr("Service 'compute_ik' call failed: %s", str(e)) return None
def requestIK(link_name, group_name, p, o, compute_ik): px, py, pz = p ox, oy, oz, ow = o request = GetPositionIKRequest() request.ik_request.group_name = group_name request.ik_request.ik_link_name = link_name request.ik_request.attempts = 10000 request.ik_request.pose_stamped.header.frame_id = "head_camera" request.ik_request.pose_stamped.pose.position.x = px request.ik_request.pose_stamped.pose.position.y = py request.ik_request.pose_stamped.pose.position.z = pz request.ik_request.pose_stamped.pose.orientation.x = ox request.ik_request.pose_stamped.pose.orientation.y = oy request.ik_request.pose_stamped.pose.orientation.z = oz request.ik_request.pose_stamped.pose.orientation.w = ow try: # print request print 'finding ik....' response = compute_ik(request) print 'moveIt succeeded' # print(response) group = MoveGroupCommander(group_name) group.set_pose_target(request.ik_request.pose_stamped) group.go() except rospy.ServiceException, e: print "Service call failed: %s" % e
def move_action(x): #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) step = 0 pose = [x] if step == 0: s = pose[step] request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "ar_marker_0" request.ik_request.pose_stamped.pose.position.x = s[0] request.ik_request.pose_stamped.pose.position.y = s[1] request.ik_request.pose_stamped.pose.position.z = s[2] request.ik_request.pose_stamped.pose.orientation.x = s[3] request.ik_request.pose_stamped.pose.orientation.y = s[4] request.ik_request.pose_stamped.pose.orientation.z = s[5] request.ik_request.pose_stamped.pose.orientation.w = s[6] response = compute_ik(request) group = MoveGroupCommander("left_arm") group.set_pose_target(request.ik_request.pose_stamped) group.go() step = step + 1
def ik_solve(self, pose, feed_joint_state=[0.0, 0.0, 0.0, 0.0, 0.0]): """ Given a end-effector pose, use inverse kinematics to determine the nessecary joint angles to reach the pose. CRED: https://github.com/uts-magic-lab/moveit_python_tools/blob/master/src/moveit_python_tools/get_ik.py """ # Build the the service request req = GetPositionIKRequest() req.ik_request.group_name = self.planning_group req.ik_request.robot_state.joint_state.name = self.joint_names req.ik_request.robot_state.joint_state.position = feed_joint_state req.ik_request.avoid_collisions = True req.ik_request.ik_link_name = self.eef_link req.ik_request.pose_stamped = pose req.ik_request.timeout.secs = 1.0 req.ik_request.timeout.nsecs = 0.0 req.ik_request.attempts = 0 # Try to send the request to the 'compute_ik' service try: resp = self.ik_srv.call(req) return resp except rospy.ServiceException: rospy.logerr("Service execption: " + str(rospy.ServiceException))
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5), verbose=True): """Computes inverse kinematics for the given pose. Note: if you are interested in returning the IK solutions, we have shown how to access them. Args: pose_stamped: geometry_msgs/PoseStamped. timeout: rospy.Duration. How long to wait before giving up on the IK solution. Returns: True if the inverse kinematics were found, False otherwise. """ request = GetPositionIKRequest() request.ik_request.pose_stamped = pose_stamped request.ik_request.group_name = 'arm' request.ik_request.timeout = timeout response = self._compute_ik(request) error_str = moveit_error_string(response.error_code.val) success = error_str == 'SUCCESS' if not success: return False if verbose: joint_state = response.solution.joint_state for name, position in itertools.izip(joint_state.name, joint_state.position): if name in ArmJoints.names(): rospy.loginfo('{}: {}'.format(name, position)) return True
def compute_inverse_kinematics(self, end_effector_pose): ''' compute the inverse kinematics of the given end effector pose,return joint values,end_effector_pose should be a pose ''' request = GetPositionIKRequest() request.ik_request.group_name = self.group_name request.ik_request.ik_link_name = "wrist_3_link" request.ik_request.attempts = 1000 request.ik_request.pose_stamped.header.frame_id = "first_link" request.ik_request.pose_stamped.pose.position.x = end_effector_pose.position.x request.ik_request.pose_stamped.pose.position.y = end_effector_pose.position.y request.ik_request.pose_stamped.pose.position.z = end_effector_pose.position.z request.ik_request.pose_stamped.pose.orientation.x = end_effector_pose.orientation.x request.ik_request.pose_stamped.pose.orientation.y = end_effector_pose.orientation.y request.ik_request.pose_stamped.pose.orientation.z = end_effector_pose.orientation.z request.ik_request.pose_stamped.pose.orientation.w = end_effector_pose.orientation.w #print(request) ik_response = self.compute_ik(request) #print(ik_response) joint_value = ik_response.solution.joint_state.position joint_values = [] if len(joint_value) < 6: #rospy.logerr('the given end_effector_pose has no results') return joint_values else: for i in range(len(joint_value)): joint_values.append(joint_value[i]) #print(joint_value) return joint_values
def main(): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while not rospy.is_shutdown(): ## get the (x,y,z) from the user while True: position = raw_input( 'Press enter to compute an IK solution:(x,y,z)') position = position.split() if len(position) != 3: print "error: Please enter 3 floats" else: position = [float(i) for i in position] break #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = position[0] request.ik_request.pose_stamped.pose.position.y = position[1] request.ik_request.pose_stamped.pose.position.z = position[2] request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request) #Print the response HERE #print(response) #print(response.error_code) #print(response.solution) error_code = response.error_code.val if error_code != 1: print("Get solution error with code %d\n" % ((error_code), )) else: print("position input (%f,%f,%f)" % tuple(position)) print("Solution get:") joint_state = response.solution.joint_state #print(joint_state) print("Result by forward_kinematics") lab3_helper.forward_kinematics(joint_state) except rospy.ServiceException, e: print "Service call failed: %s" % e
def main(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("ik_moveit") pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=1) compute_ik_srv = rospy.ServiceProxy("/compute_ik", GetPositionIK) rospy.wait_for_service("/compute_ik") arm_move_group = moveit_commander.MoveGroupCommander("manipulator") robot_commander = moveit_commander.RobotCommander() # compute reference position of the gripper ( link) ref_pose = pq_to_pose([0.256, 0.294, 0.365], [0.707, 0.707, 0.0, 0.0]) rate = rospy.Rate(10.0) iter_no = 0 max_iter = 10 while not rospy.is_shutdown(): # set reference position of the gripper ( link) pose_ik = PoseStamped() pose_ik.header.frame_id = "base_link" pose_ik.header.stamp = rospy.Time.now() pose_ik.pose = ref_pose req = GetPositionIKRequest() req.ik_request.group_name = "manipulator" req.ik_request.robot_state = robot_commander.get_current_state() req.ik_request.avoid_collisions = True req.ik_request.ik_link_name = arm_move_group.get_end_effector_link() req.ik_request.pose_stamped = pose_ik # print(arm_move_group.get_end_effector_link()) ik_response = compute_ik_srv(req) if ik_response.error_code.val == 1: print('Goal state:') print(ik_response.solution.joint_state.position) goal_pose = FollowJointTrajectoryActionGoal() # print (arm_move_group.get_joints()) goal_pose.goal.trajectory.joint_names = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] goal_pose.goal.trajectory.points.append(JointTrajectoryPoint()) goal_pose.goal.trajectory.points[ 0].positions = ik_response.solution.joint_state.position goal_pose.goal.trajectory.points[0].velocities = [0, 0, 0, 0, 0, 0] goal_pose.goal.trajectory.points[ 0].time_from_start = rospy.Duration.from_sec(1.0) pub.publish(goal_pose) else: print('Could not find solution to inverse kinematic') rate.sleep() iter_no = iter_no + 1 if iter_no > max_iter: break
def _do_ik(self, px, py, pz, arm): """ Does inverse kinematics for a given arm and given position. This solver is collision aware. Args: px, py, pz (float): Cartesian coordinates for position of arm end effector. arm (str): Which arm ("right_arm" or "left_arm") to do ik for. Returns: list: List of joint configuration for specific arm. Returns an empty list if unable to do IK. """ IK_INFO_NAME = "/pr2_%s_kinematics/get_ik_solver_info" % (arm) IK_NAME = "/compute_ik" ik_solver_info_service_proxy = rospy.ServiceProxy(IK_INFO_NAME, GetKinematicSolverInfo) ik_info_req = GetKinematicSolverInfoRequest() rospy.loginfo("LightningTest: do_ik: Waiting for %s" % (IK_INFO_NAME)) rospy.wait_for_service(IK_INFO_NAME) rospy.loginfo("About to get IK info.") ik_info_res = ik_solver_info_service_proxy(ik_info_req) rospy.loginfo("Got IK Solver info.") ik_solver_service_proxy = rospy.ServiceProxy(IK_NAME, GetPositionIK) ik_solve_req = GetPositionIKRequest() ik_solve_req.ik_request.group_name = arm ik_solve_req.ik_request.timeout = rospy.Duration(5.0) ik_solve_req.ik_request.ik_link_name = "%s_wrist_roll_link" % (arm[0]) ik_solve_req.ik_request.pose_stamped.header.frame_id = "odom_combined" ik_solve_req.ik_request.pose_stamped.pose.position.x = px ik_solve_req.ik_request.pose_stamped.pose.position.y = py ik_solve_req.ik_request.pose_stamped.pose.position.z = pz ik_solve_req.ik_request.pose_stamped.pose.orientation.x = 0.0; ik_solve_req.ik_request.pose_stamped.pose.orientation.y = 0.0; ik_solve_req.ik_request.pose_stamped.pose.orientation.z = 0.0; ik_solve_req.ik_request.pose_stamped.pose.orientation.w = 1.0; # TODO: Consider retrieving current robto state. ik_solve_req.ik_request.robot_state.joint_state.name = ik_info_res.kinematic_solver_info.joint_names; for i in xrange(len(ik_info_res.kinematic_solver_info.joint_names)): joint_limits = ik_info_res.kinematic_solver_info.limits[i] ik_solve_req.ik_request.robot_state.joint_state.position.append((joint_limits.min_position + joint_limits.max_position)/2.0) joint_constraint = JointConstraint() joint_constraint.joint_name = ik_info_res.kinematic_solver_info.joint_names[i] joint_constraint.position = (joint_limits.max_position + joint_limits.min_position) / 2.0 joint_constraint.tolerance_above = (joint_limits.max_position - joint_limits.min_position) / 2.0 joint_constraint.tolerance_below = joint_constraint.tolerance_above ik_solve_req.ik_request.constraints.joint_constraints.append(joint_constraint) rospy.loginfo("LightningTest: do_ik: Waiting for IK service.") rospy.wait_for_service(IK_NAME) ik_solve_res = ik_solver_service_proxy(ik_solve_req) if ik_solve_res.error_code.val == ik_solve_res.error_code.SUCCESS: return ik_solve_res.solution.joint_state.position[30:37] else: rospy.loginfo("Lightning tester: cannot move to sampled position...trying another position") return []
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. self._move_group = userdata.move_group self._move_group_prefix = userdata.move_group_prefix self._tool_link = userdata.tool_link self._offset = userdata.offset self._rotation = userdata.rotation self._srv_name = userdata.move_group_prefix + '/compute_ik' self._ik_srv = ProxyServiceCaller({self._srv_name: GetPositionIK}) self._robot1_client = actionlib.SimpleActionClient(userdata.move_group_prefix + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self._robot1_client.wait_for_server() rospy.loginfo('Execute Trajectory server is available for robot') # Get transform between camera and robot1_base while True: rospy.sleep(0.1) try: target_pose = self._tf_buffer.transform(userdata.pose, "world") break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logerr("ComputeGraspState::on_enter - Failed to transform to world") continue # the grasp pose is defined as being located on top of the item target_pose.pose.position.z += self._offset + 0.0 # rotate the object pose 180 degrees around - now works with -90??? #q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w] q_orig = [0, 0, 0, 1] #q_rot = quaternion_from_euler(self._rotation, 0, 0) q_rot = quaternion_from_euler(self._rotation, math.pi/2.0, 0) # math.pi/2.0 added by gerard!! #q_rot = quaternion_from_euler(math.pi/-2.0, 0, 0) res_q = quaternion_multiply(q_rot, q_orig) target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q) # use ik service to compute joint_values self._srv_req = GetPositionIKRequest() self._srv_req.ik_request.group_name = self._move_group self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(self._move_group_prefix + '/joint_states', sensor_msgs.msg.JointState) self._srv_req.ik_request.ik_link_name = self._tool_link # TODO: this needs to be a parameter self._srv_req.ik_request.pose_stamped = target_pose self._srv_req.ik_request.avoid_collisions = True self._srv_req.ik_request.attempts = 500 try: self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req) self._failed = False except Exception as e: Logger.logwarn('Could not call IK: ' + str(e)) self._failed = True
def check_ee_valid_pose(self,action): #checking the validity of the end effector pose #converting to joint state using ik and then getting the validity GPIK_Request =GetPositionIKRequest() GPIK_Request.group_name = GPIK_Request.group_name = 'arm_controller' GPIK_Request.robot_state.joint_state = self.joints GPIK_Request.avoid_collisions = True # this poses is related to the reference frame of gazebo # the pose is set as a radian value between 1.571 and -1.571 GPIK_Request.pose_stamped.pose.position.x = action[ 0 ] GPIK_Request.pose_stamped.pose.position.y = action[ 1 ] GPIK_Request.pose_stamped.pose.position.z = action[ 2 ] GPIK_Request.pose_stamped.pose.orientation = quaternion_from_euler([0, 1.571, action[3]]) GPIK_Response = self.joint_state_from_pose_client(GPIK_Request) if GPIK_Response.error_code == 1: return True else: return GPIK_Response.error_code
def get_position_ik(robot_state, ik_link_name, pose_stamped, constraints=Constraints(), avoid_collisions=True, timeout=rospy.Time(1), persistent=False, wait_timeout=None): """Call position IK service. INPUT (see moveit_msgs/PositionIKRequest for details) robot_state [moveit_msgs/RobotState] ik_link_name [String, default=""] pose_stamped [geometry_msgs/PoseStamped, default=None] constraints [moveit_msgs/Constraints, default=None] avoid_collisions [Bool, default=True] timeout [Float, default=1.0] persistent [Bool, default=False] Set to True if calling this function many times. wait_timeout [float, default=None] Time to wait for the service. OUTPUT response [moveit_msgs/GetPositionIKResponse] """ ik_srv = get_service_proxy(SRV_GET_POSITION_IK_SERVICE, GetPositionIK, persistent, wait_timeout) # Make a copy pose_stamped = deepcopy(pose_stamped) # Fix the link if differ from default new_link_name, new_pose = utils.fix_link_for_ik(pose_stamped.pose, ik_link_name) if not new_link_name == ik_link_name: rospy.logdebug("Link %s is not IK link, was converted to %s", ik_link_name, new_link_name) ik_link_name = new_link_name pose_stamped.pose = new_pose req = GetPositionIKRequest() req.ik_request.group_name = infer_group_from_ee(ik_link_name) req.ik_request.robot_state = robot_state req.ik_request.constraints = constraints req.ik_request.avoid_collisions = avoid_collisions req.ik_request.ik_link_name = ik_link_name req.ik_request.pose_stamped = pose_stamped req.ik_request.ik_link_names = [] req.ik_request.pose_stamped_vector = [] req.ik_request.timeout = timeout req.ik_request.attempts = 1 res = ik_srv(req) return res
def main(x, y, z): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while not rospy.is_shutdown(): raw_input('Press enter to compute an IK solution:') #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = x request.ik_request.pose_stamped.pose.position.y = y request.ik_request.pose_stamped.pose.position.z = z request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request) message = response.solution.joint_state #### #Print the contents of the message to the console theta = [] theta.append(message.position[1]) theta.append(message.position[2]) theta.append(message.position[3]) theta.append(message.position[4]) theta.append(message.position[5]) theta.append(message.position[6]) theta.append(message.position[7]) print("theta = ") print(theta) print("\n") print("------") print("gst = ") print(lab3.lab3(theta)) print("--------------") print(response) #Print the response HERE #### except rospy.ServiceException, e: print "Service call failed: %s" % e
def main(robo): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') arm = 'left' #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) if robo == 'sawyer': arm = 'right' while not rospy.is_shutdown(): raw_input('Press [ Enter ]: ') #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = arm + "_arm" #Alan does not have a gripper so replace link with 'right_wrist' instead link = arm + "_gripper" if robo == 'sawyer': link += '_tip' request.ik_request.ik_link_name = link request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = 0.5 request.ik_request.pose_stamped.pose.position.y = 0.5 request.ik_request.pose_stamped.pose.position.z = 0.0 request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request) #Print the response HERE print(response) group = MoveGroupCommander(arm + "_arm") # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation ###group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() except rospy.ServiceException, e: print "Service call failed: %s" % e
def get_ik(self, pose_stamped, group=None, ik_timeout=None, ik_attempts=None, avoid_collisions=None): """ Do an IK call to pose_stamped pose. geometry_msgs/PoseStamped pose_stamped: The 3D pose (with header.frame_id) to which compute the IK. Parameters ---------- pose_stamped : :obj:`geometry_msgs.msg.PoseStamped` goal pose group : string The MoveIt! group. ik_timeout : float The timeout for the IK call. ik_attemps : int The maximum # of attemps for the IK. avoid_collisions : bool If to compute collision aware IK. Returns ------- moveit_msgs.srv.GetPositionIKResponse Response from /compute_ik service """ if group is None: group = self.group_name if ik_timeout is None: ik_timeout = self.ik_timeout if ik_attempts is None: ik_attempts = self.ik_attempts if avoid_collisions is None: avoid_collisions = self.avoid_collisions req = GetPositionIKRequest() req.ik_request.group_name = group req.ik_request.pose_stamped = pose_stamped req.ik_request.timeout = rospy.Duration(ik_timeout) req.ik_request.attempts = ik_attempts req.ik_request.avoid_collisions = avoid_collisions try: resp = self.ik_srv.call(req) return resp except rospy.ServiceException as e: rospy.logerr("Service exception: " + str(e)) resp = GetPositionIKResponse() resp.error_code = 99999 # Failure return resp
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. # Get transform between camera and robot1_base while True: rospy.sleep(0.1) try: target_pose = self._tf_buffer.transform(userdata.pose, "world") break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logerr( "ComputeGraspState::on_enter - Failed to transform to world" ) continue # the grasp pose is defined as being located on top of the box target_pose.pose.position.z += self._offset + 0.0 # rotate the object pose 180 degrees around - now works with -90??? #q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w] q_orig = [0, 0, 0, 1] q_rot = quaternion_from_euler(self._rotation, 0, 0) #q_rot = quaternion_from_euler(math.pi/-2.0, 0, 0) res_q = quaternion_multiply(q_rot, q_orig) target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q) # use ik service to compute joint_values self._srv_req = GetPositionIKRequest() self._srv_req.ik_request.group_name = self._group self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message( self._group + '/joint_states', sensor_msgs.msg.JointState) self._srv_req.ik_request.ik_link_name = self._tool_link # TODO: this needs to be a parameter self._srv_req.ik_request.pose_stamped = target_pose self._srv_req.ik_request.avoid_collisions = True self._srv_req.ik_request.attempts = 500 for tries in range(0, 3): try: self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req) self._failed = False Logger.logwarn('ComputeGraspState: attempt ' + str(tries)) if int(self._srv_result.error_code.val) == 1: break except Exception as e: Logger.logwarn('Could not call IK: ' + str(e)) self._failed = True
def main(): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while not rospy.is_shutdown(): raw_input('Press [ Enter ]: ') #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 20 #request.ik_request.pose_stamped.header.frame_id = "base" request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE euler_angle = [-np.pi, 0, 0] traslation = [0.591, 0.261, 0.327 - 0.1] quaternion = tf.transformations.quaternion_from_euler( euler_angle[0], euler_angle[1], euler_angle[2]) request.ik_request.pose_stamped.pose.position.x = traslation[0] request.ik_request.pose_stamped.pose.position.y = traslation[1] request.ik_request.pose_stamped.pose.position.z = traslation[2] request.ik_request.pose_stamped.pose.orientation.x = quaternion[0] request.ik_request.pose_stamped.pose.orientation.y = quaternion[1] request.ik_request.pose_stamped.pose.orientation.z = quaternion[2] request.ik_request.pose_stamped.pose.orientation.w = quaternion[3] try: #Send the request to the service response = compute_ik(request) #Print the response HERE #print(response) group = MoveGroupCommander("left_arm") # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation ###group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() except rospy.ServiceException, e: print "Service call failed: %s" % e
def move_to(self, pos_x, pos_y, pos_z, orien_x, orien_y, orien_z, orien_w, ik, orien_const=None): #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = pos_x request.ik_request.pose_stamped.pose.position.y = pos_y request.ik_request.pose_stamped.pose.position.z = pos_z request.ik_request.pose_stamped.pose.orientation.x = orien_x request.ik_request.pose_stamped.pose.orientation.y = orien_y request.ik_request.pose_stamped.pose.orientation.z = orien_z request.ik_request.pose_stamped.pose.orientation.w = orien_w try: #Send the request to the service response = ik(request) #Print the response HERE print(response) group = MoveGroupCommander("right_arm") if orien_const is not None: constraints = Constraints() constraints.orientation_constraints = orien_const group.set_path_constraints(constraints) # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation # group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() except rospy.ServiceException, e: print("Service call failed: %s")
def move_group_pick(self, compute_ik, transformation_mat): t = geometry_msgs.msg.Pose() position = tf.transformations.translation_from_matrix( transformation_mat) orientation = tf.transformations.quaternion_from_matrix( transformation_mat) t.position.x = position[0] t.position.y = position[1] t.position.z = position[2] t.orientation.x = orientation[0] t.orientation.y = orientation[1] t.orientation.z = orientation[2] t.orientation.w = orientation[3] # With this function, we will be able t achieve the pre grasp position # scene = PlanningSceneInterface() # p = PoseStamped() # start = rospy.get_time() # seconds = rospy.get_time() # box_name = "obj1" # while (seconds - start < 5) and not rospy.is_shutdown(): # # Test if the box is in attached objects # attached_objects = scene.get_attached_objects([box_name]) # is_attached = len(attached_objects.keys()) > 0 # is_known = box_name in scene.get_known_object_names() # seconds = rospy.get_time() self.group.set_named_target("initial") self.group.go() request = GetPositionIKRequest() request.ik_request.group_name = "arm" request.ik_request.ik_link_name = "wrist_3_link" request.ik_request.attempts = 20 #request.ik_request.pose_stamped.header.frame_id = "base" request.ik_request.pose_stamped.header.frame_id = "ur10_base_link" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = t.position.x - 0.2 request.ik_request.pose_stamped.pose.position.y = t.position.y request.ik_request.pose_stamped.pose.position.z = t.position.z - 0.4 request.ik_request.pose_stamped.pose.orientation.x = -0.707 request.ik_request.pose_stamped.pose.orientation.y = 0 request.ik_request.pose_stamped.pose.orientation.z = 0 request.ik_request.pose_stamped.pose.orientation.w = 0.707 response = compute_ik(request) group = self.group group.set_pose_target(request.ik_request.pose_stamped) group.go() self.gripper.set_named_target("close") self.gripper.go() self.group.set_named_target("initial") self.group.go()
def compute_moveit_ik(end_effector_pose, seed, group="right_arm"): req = GetPositionIKRequest() req.ik_request.group_name = group arm = "l" if group == "left_arm" else "r" if group == "left_arm": req.ik_request.robot_state.joint_state.name = ['yumi_joint_%d_l' % i for i in [1, 2, 7, 3, 4, 5, 6]] else: req.ik_request.robot_state.joint_state.name = ['yumi_joint_%d_r' % i for i in [1, 2, 7, 3, 4, 5, 6]] req.ik_request.robot_state.joint_state.position = seed req.ik_request.pose_stamped = end_effector_pose req.ik_request.timeout = rospy.Duration(0.05) req.ik_request.attempts = 20 res = ik_srv(req).solution return res.joint_state.position
def movement(des_coor, i): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') received_info = True #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while received_info == True: #Construct the requests request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 30 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE print(des_coor) request.ik_request.pose_stamped.pose.position.x = des_coor[0] request.ik_request.pose_stamped.pose.position.y = des_coor[1] request.ik_request.pose_stamped.pose.position.z = 0.25 + initial_xyz[ 2] # this should be a found constant # Should be constant Determined by Experiment request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request) #Print the response HERE print(response) if response.error_code.val == 1: received_info = False else: received_info = False # received_info = False group = MoveGroupCommander("left_arm") # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # Plan IK and execute group.go() except rospy.ServiceException, e: print "Service call failed: %s" % e
def discard(self, poses): validp = [] validrs = [] req = GetPositionIKRequest() req.ik_request.group_name = self.arm_move_group_name req.ik_request.robot_state = self.robot_commander.get_current_state() req.ik_request.avoid_collisions = True for p in poses: req.ik_request.pose_stamped = p k = self.compute_ik_srv(req) if k.error_code.val == 1: validp.append(p) validrs.append(k.solution) if validp: return [validp, validrs] return []
def get_position_ik(self, position, orientation): rospy.wait_for_service(self.ns + 'solve_ik') req = GetPositionIKRequest() 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] result = self._get_position_ik(req) if result.error_code.val == result.error_code.SUCCESS: return list(result.solution.joint_state.position) else: return []
try: joint_states_client = rospy.ServiceProxy('return_joint_states', ReturnJointStates) resp = joint_states_client() current_joint_position = resp.position except rospy.ServiceException, e: print "error when calling return_joint_states: %s"%e try: query_client = rospy.ServiceProxy('pr2_right_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo) resp = query_client() joint_names = resp.kinematic_solver_info.joint_names limits = resp.kinematic_solver_info.limits except rospy.ServiceException, e: print 'Cannot call ik solver query service'%e try: ik_client = rospy.ServiceProxy('/pr2_right_arm_kinematics/get_ik', GetPositionIK) req = GetPositionIKRequest() req.ik_request.ik_link_name = "r_wrist_roll_link" req.ik_request.pose_stamped = pose_stamped req.ik_request.ik_seed_state.joint_state.name = joint_names req.ik_request.ik_seed_state.joint_state.position = current_joint_position req.timeout = rospy.Duration(5.0) resp = ik_client(req) if resp.error_code.val == resp.error_code.SUCCESS: print 'Found IK Solution:', resp.solution.joint_state.position return (resp.solution.joint_state.position, True) else: print 'error code: ', resp.error_code.val return ([], False) except rospy.ServiceException, e: print "Service call failed: %s"%e return ([], False)