def solveIK(self, des_pose): ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') pose = Pose() quat = Quaternion() if len(des_pose) == 6: #Note that this may be wrong! quatArray = quaternion_from_euler(des_pose[3], des_pose[4], des_pose[5], axes='sxyz') quat.x = quatArray[0] quat.y = quatArray[1] quat.z = quatArray[2] quat.w = quatArray[3] else: quat.x = des_pose[3] quat.y = des_pose[4] quat.z = des_pose[5] quat.w = des_pose[6] pose.orientation = quat pose.position.x = des_pose[0] pose.position.y = des_pose[1] pose.position.z = des_pose[2] # print pose.position ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] try: rospy.wait_for_service(self.ns, 5.0) resp = self.iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("IK Service Exception thrown") return None
def main(): rospy.init_node("scan_initial") #Create a publisher right = baxter_interface.Limb('right') # Call service ns_r = "ExternalTools/" + 'right' + "/PositionKinematicsNode/IKService" iksvc_r = rospy.ServiceProxy(ns_r, SolvePositionIK) ikreq_r = SolvePositionIKRequest() # Customize message hdr = Header(stamp=rospy.Time.now(), frame_id='base') #z-axis and orientation must be kept in this form for baxter to reach outward poses = { 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=0.85, y=-0.39, z=-0.00, ), orientation=Quaternion( x=0.00, y=0.71, z=0.0, w=0.71, ), ), ), } ikreq_r.pose_stamp = [poses['right']] print ikreq_r try: rospy.wait_for_service(ns_r, 5.0) resp_r = iksvc_r(ikreq_r) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return
def move_pos(goal, limb, timeout=3): # Call Baxter's inbuilt service ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) # Request for same service ikreq = SolvePositionIKRequest() # Fill in all message parts hdr = Header(stamp=rospy.Time.now(), frame_id='base') # Define required x, y, z parts pose = Pose() # Define orientation of gripper. quat = Quaternion() quat.x = goal[3] quat.y = goal[4] quat.z = goal[5] quat.w = goal[6] # Fill the position vectors from goal configuration pose.orientation = quat pose.position.x = goal[0] pose.position.y = goal[1] pose.position.z = goal[2] ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] # Using Random seeds till a "VALID" IK solution is found # FOR SEED METHOD: # cite1: IK_Service_Client in package: Baxter_Examples # cite2: test_moveit.py from NXR repo linked in Course notes try: rospy.wait_for_service(ns, 3.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.loginfo("Service exception")
def move_pos(des_pose): limb = "right" ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5]) pose = Pose() quat = Quaternion() quat.x = des_pose[3] quat.y = des_pose[4] quat.z = des_pose[5] quat.w = des_pose[6] pose.orientation = quat pose.position.x = des_pose[0] pose.position.y = des_pose[1] pose.position.z = des_pose[2] ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] # print ikreq try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.loginfo("Service exception")
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5]) pose = Pose() quat = Quaternion() quat.x = des_pose[3] quat.y = des_pose[4] quat.z = des_pose[5] quat.w = des_pose[6] pose.orientation = quat pose.position.x = des_pose[0] pose.position.y = des_pose[1] pose.position.z = des_pose[2] ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] # print ikreq try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.loginfo("Service exception") if resp.isValid[0]: des_joints = [0]*7 for i in range(7): des_joints[i] = resp.joints[0].position[i] # print right_arm_group.get_current_pose() # print des_joints right_arm_group.set_joint_value_target(des_joints)
def move_callback(data): #set object size force parameters side = data.side # print side point = data.point # print point quaternion = data.quaternion # print quaternion speed = data.speed # print speed npoints = data.npoints ns = "ExternalTools/" + side + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() arm = baxter_interface.Limb(side) arm.set_joint_position_speed(0.5 * speed) x, y, z = point.x, point.y, point.z if side == 'right': cur_x = position_now_R.x cur_y = position_now_R.y cur_z = position_now_R.z elif side == 'left': cur_x = position_now_L.x cur_y = position_now_L.y cur_z = position_now_L.z else: raise ValueError( "INVALID SIDE PASSED TO MOVE.PY, OPTIONS ARE 'left' OR 'right'") dx = x - cur_x dy = y - cur_y dz = z - cur_z print 'dx, dy, dz', dx, dy, dz if npoints == 0: dist = np.sqrt(dx**2 + dy**2 + dz**2) npoints = int(dist / 0.01) # print npoints xs = np.linspace(x - dx, x, npoints, endpoint=True) ys = np.linspace(y - dy, y, npoints, endpoint=True) zs = np.linspace(z - dz, z, npoints, endpoint=True) for i in np.arange(npoints): pose = { side: PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=Pose( position=Point( # x = x[i], # y = y[i], # z = z[i], x=xs[i], y=ys[i], z=zs[i], ), # e-e must be maintained in this orientation orientation=Quaternion( x=quaternion.x, y=quaternion.y, z=quaternion.z, w=quaternion.w, ), ), ), } ikreq = SolvePositionIKRequest() ikreq.pose_stamp = [pose[side]] count = 0 while count < 3: try: resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return if (resp.isValid[0]): limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) # arm.set_joint_positions(limb_joints) if i == npoints - 1 and npoints > 10 or npoints == 1: # SWAG arm.move_to_joint_positions(limb_joints) else: arm.set_joint_positions(limb_joints) rospy.sleep(0.025 / speed) break else: print("INVALID POSE - No Valid Joint Solution Found.") count += 1
while not rospy.is_shutdown(): tr.header.stamp = rospy.Time.now() br.sendTransform(tr) # update pose Md[0, 3] = 0.05 * np.cos(t / 2.) Md[1, 3] = 0.1 * np.sin(t / 2) Md[2, 3] = 0.2 * np.cos(t / 2) Md[:3, :3] = transformations.euler_matrix(.1 * np.cos(t), .1 * np.sin(t / 2), .05 * np.cos(t + 4))[:3, :3] M = np.dot(M0, Md) req.pose_stamp = [matrix_msg(M)] res = ikr(req) cmd.names = res.joints[0].name cmd.command = res.joints[0].position pubr.publish(cmd) req.pose_stamp = [matrix_msg(np.dot(M, Mrl))] res = ikl(req) cmd.names = res.joints[0].name cmd.command = res.joints[0].position publ.publish(cmd) t += dt rospy.sleep(dt)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') # quat = quaternion_from_euler(des_pose[3],des_pose[4],des_pose[5]) pose = Pose() quat = Quaternion() quat.x = des_pose[3] quat.y = des_pose[4] quat.z = des_pose[5] quat.w = des_pose[6] pose.orientation = quat pose.position.x = des_pose[0] pose.position.y = des_pose[1] pose.position.z = des_pose[2] ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)] # print ikreq try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.loginfo("Service exception") if resp.isValid[0]: des_joints = [0] * 7 for i in range(7): des_joints[i] = resp.joints[0].position[i] # print right_arm_group.get_current_pose() # print des_joints right_arm_group.set_joint_value_target(des_joints)
x=0.85, y=0.00, z=0.00, ), orientation=Quaternion( x=0.0, y=0.71, z=0.0, w=-0.71, ), ), ), } ikreq_r = SolvePositionIKRequest() ikreq_r.pose_stamp = [poses['right']] seed = random_seed() ikreq_r.seed_angles = seed ikreq_r.seed_mode = 1 print "IKREQ_FIXED = ", ikreq_r #print "IKREQ_R : ",ikreq_r try: rospy.wait_for_service(ns_r, 5.0) resp_r = iksvc_r(ikreq_r) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return if (resp_r.isValid[0]): print("RIGHT_SUCCESS - Valid Joint Solution Found:")